#include "commonfunctionset.h"
#include <QTimer>
#include <QDataStream>
#include <QEventLoop>
#include <Eigen/Dense> 
#include <Eigen/Core>
#include <Eigen/Geometry>


#ifndef M_PI
#define M_PI (3.14159265358979323846)
#endif
CommonFun::CommonFun()
{}

// 欧拉角转换为3*3旋转矩阵
Eigen::Matrix3d TransEuler2Matrix(float Za, float Ya, float Xa)
{
	double yaw = Za / 180 * M_PI;
	double roll = Ya / 180 * M_PI;
	double pitch = Xa / 180 * M_PI;
	Eigen::Vector3d Euler(yaw, roll, pitch);
	Eigen::Matrix3d dRotationMatrix;

	dRotationMatrix = Eigen::AngleAxisd(Euler[0], Eigen::Vector3d::UnitZ()) *
		Eigen::AngleAxisd(Euler[1], Eigen::Vector3d::UnitY()) *
		Eigen::AngleAxisd(Euler[2], Eigen::Vector3d::UnitX());
	return dRotationMatrix;
}

// 欧拉角坐标转换为4维旋转矩阵
Eigen::Matrix4d TransEulerPostion2Matrix(const Position& pos)
{
	Eigen::Matrix3d dRotationMatrix = TransEuler2Matrix(pos.a, pos.b, pos.c);
	Eigen::Matrix4d posMat = Eigen::Matrix4d::Identity();
	posMat.block(0, 0, 3, 3) = dRotationMatrix;
	posMat(0, 3) = pos.x;
	posMat(1, 3) = pos.y;
	posMat(2, 3) = pos.z;
	return posMat;
}



Position CommonFun::Quat2Eul(const PositionQuaternion& input)
{
	Position out = {};
	out.x = input.x;
	out.y = input.y;
	out.z = input.z;

	Eigen::Quaterniond quat(input.Q1, input.Q2, input.Q3, input.Q4);
	Eigen::Vector3d eulangle = quat.matrix().eulerAngles(2, 1, 0);
	std::vector<double> res;
	res.push_back(eulangle[0] * 180 / M_PI);
	res.push_back(eulangle[1] * 180 / M_PI);
	res.push_back(eulangle[2] * 180 / M_PI);
	if (res[1] != 0) {              // 机械手要求坐标y轴旋转一定为0
		res[0] = (res[0] + 180);
		if (res[0] >= 360) res[0] = res[0] - 360;
		if (res[0] <= -360) res[0] = res[0] + 360;
		if (res[0] > 180) res[0] = res[0] - 360;
		if (res[0] < -180) res[0] = res[0] + 360;

		res[1] = (180 - res[1]);
		if (res[1] >= 360) res[1] = res[1] - 360;
		if (res[1] <= -360) res[1] = res[1] + 360;
		if (res[1] > 180) res[1] = res[1] - 360;
		if (res[1] < -180) res[1] = res[1] + 360;

		res[2] = (res[2] + 180);
		if (res[2] > 360) res[2] = res[2] - 360;
		if (res[2] <= -360) res[2] = res[2] + 360;
		if (res[2] > 180) res[2] = res[2] - 360;
		if (res[2] < -180) res[2] = res[2] + 360;
	}

	out.a = (float)res[0];
	out.b = (float)res[1];
	out.c = (float)res[2];

	return out;
}

PositionQuaternion CommonFun::Eul2Quat(const Position& input)
{
	PositionQuaternion out = {};
	out.x = input.x;
	out.y = input.y;
	out.z = input.z;
	Eigen::Vector3d ea(input.a * M_PI / 180.0, input.b * M_PI / 180.0, input.c * M_PI / 180.0);
	Eigen::Matrix3d R;
	R = Eigen::AngleAxisd(ea[0], ::Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(ea[1], ::Eigen::Vector3d::UnitY())
		* Eigen::AngleAxisd(ea[2], ::Eigen::Vector3d::UnitX());
	Eigen::Quaterniond q(R);

	out.Q1 = std::abs(q.w()) < 0.0001 ? 0.0f : (float)q.w();
	out.Q2 = std::abs(q.x()) < 0.0001 ? 0.0f : (float)q.x();
	out.Q3 = std::abs(q.y()) < 0.0001 ? 0.0f : (float)q.y();
	out.Q4 = std::abs(q.z()) < 0.0001 ? 0.0f : (float)q.z();
	return out;
}

inline PositionQuaternion Matrix4dToQuaternion(const Eigen::Matrix4d& in)
{
	PositionQuaternion out = {};

	out.x = (float)in(0, 3);
	out.y = (float)in(1, 3);
	out.z = (float)in(2, 3);
	Eigen::Matrix3d mat = in.block<3, 3>(0, 0);
	Eigen::Quaterniond q(mat);
	out.Q1 = std::abs(q.w()) < 0.0001 ? 0.0f : (float)q.w();
	out.Q2 = std::abs(q.x()) < 0.0001 ? 0.0f : (float)q.x();
	out.Q3 = std::abs(q.y()) < 0.0001 ? 0.0f : (float)q.y();
	out.Q4 = std::abs(q.z()) < 0.0001 ? 0.0f : (float)q.z();
	return out;
}

inline PositionQuaternion Matrix4dToQuaternion(const Eigen::Inverse<Eigen::Matrix4d>& in)
{
	PositionQuaternion out = {};

	out.x = (float)in(0, 3);
	out.y = (float)in(1, 3);
	out.z = (float)in(2, 3);
	Eigen::Matrix3d mat = in.block<3, 3>(0, 0);
	Eigen::Quaterniond q(mat);
	out.Q1 = std::abs(q.w()) < 0.0001 ? 0.0f : (float)q.w();
	out.Q2 = std::abs(q.x()) < 0.0001 ? 0.0f : (float)q.x();
	out.Q3 = std::abs(q.y()) < 0.0001 ? 0.0f : (float)q.y();
	out.Q4 = std::abs(q.z()) < 0.0001 ? 0.0f : (float)q.z();
	return out;
}

Position CommonFun::TransAToBCoordinateSystem(const Position& a, const Position& b)
{
	PositionQuaternion out = {};
	Eigen::Matrix4d  matA = TransEulerPostion2Matrix(a);
	Eigen::Matrix4d  matB = TransEulerPostion2Matrix(b);
	Eigen::Matrix4d result = matB * matA;

	return Quat2Eul(Matrix4dToQuaternion(result));
}
Position CommonFun::TransAToBCoordinateSystem(const Position& a, const PositionQuaternion& b)
{
	Position inputb = Quat2Eul(b);
	return TransAToBCoordinateSystem(a, inputb);
}

Position CommonFun::TransAToBCoordinateSystem(const PositionQuaternion& a, const Position& b)
{
	Position inputa = Quat2Eul(a);
	return TransAToBCoordinateSystem(inputa, b);
}

Position CommonFun::TransAToBCoordinateSystem(const PositionQuaternion& a, const PositionQuaternion& b)
{
	Position inputa = Quat2Eul(a);
	Position inputb = Quat2Eul(b);
	return TransAToBCoordinateSystem(inputa, inputb);
}

PositionQuaternion CommonFun::FromAToBQuat(const Position& a)
{
	Eigen::Matrix4d  matA = TransEulerPostion2Matrix(a);
	return Matrix4dToQuaternion(matA.inverse());
}

PositionQuaternion CommonFun::FromAToBQuat(const PositionQuaternion& a)
{
	Position inputa = Quat2Eul(a);
	Eigen::Matrix4d  matA = TransEulerPostion2Matrix(inputa);
	return Matrix4dToQuaternion(matA.inverse());
}

Position CommonFun::FromAToBEuler(const Position& a)
{
	Eigen::Matrix4d  matA = TransEulerPostion2Matrix(a);
	auto in = matA.inverse();
	PositionQuaternion out = Matrix4dToQuaternion(in);
	return Quat2Eul(out);
}

Position CommonFun::FromAToBEuler(const PositionQuaternion& a)
{
	Position inputa = Quat2Eul(a);
	Eigen::Matrix4d  matA = TransEulerPostion2Matrix(inputa);
	auto out = Matrix4dToQuaternion(matA.inverse());
	return Quat2Eul(out);
}

void CommonFun::Sleep(int msec)
{
	QEventLoop loop;
	QTimer::singleShot(msec, &loop, &QEventLoop::quit);
	loop.exec();
}
unsigned int CommonFun::FromBinToInt(const QByteArray& d)
{
	unsigned int ret = 0;
	for (int index = d.length(); index > 0; index--) {
		ret *= 1 << 8;
		ret += (unsigned char)d.at(index - 1);
	}

	return ret;
}
QByteArray CommonFun::ToBin(int val, int size)
{
	QByteArray ret;
	ret.resize(size);
	unsigned char f = 0xff;
	for (int i = 0; i < size; i++) {
		ret[i] = (val >> i * 8) & f;
	}
	return ret;
}

QByteArray CommonFun::ToBin(float vale, int size)
{
	return QByteArray::fromRawData((const char*)&vale, size);
}

float CommonFun::FromBinToFloat(const QByteArray& d)
{
	if (d.size() != 4) {
		return 0.0f;
	}
	float val = 0.0f;
	QDataStream stream(d);
	stream.setByteOrder(QDataStream::LittleEndian);
	stream.setFloatingPointPrecision(QDataStream::SinglePrecision);
	stream >> val;
	return ((float)((int)val * 100)) / 100;
}

QString CommonFun::FromBinToString(const QByteArray& d)
{
	QString ret;
	for (auto i : d) {
		unsigned char f = 0x01;
		while (f) {
			ret.append(i & f ? "1" : "0");
			f = f << 1;
		}
	}
	return ret;
}
